#include <rclcpp/rclcpp.hpp> 
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/transform_stamped.hpp> 
#include <tf2/transform_datatypes.h>
#include <cmath>
class TFSubscriber : public rclcpp::Node {
public:
  TFSubscriber() : Node("tf_reader"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) {
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(100),
      std::bind(&TFSubscriber::timer_callback, this));
  }
 
private:
  void timer_callback() {
    try {
      // 查询目标坐标系变换（示例：从base_link到odom）
      auto transform = tf_buffer_.lookupTransform(
        "map", "base_link", tf2::TimePointZero);
 
      // 提取translation 
      double x = transform.transform.translation.x; 
      double y = transform.transform.translation.y; 
      double z = transform.transform.translation.z; 
 
      // 提取rotation（四元数）
      double qx = transform.transform.rotation.x; 
      double qy = transform.transform.rotation.y; 
      double qz = transform.transform.rotation.z; 
      double qw = transform.transform.rotation.w; 
 
      

           // 计算Z轴旋转角度 
           double roll, pitch, yaw;
           roll=0;
           pitch=0;
           yaw = 2*atan2(qz,qw);
      RCLCPP_INFO(this->get_logger(), 
        "Translation: [%.2f, %.2f, %.2f]\nRotation: [%.2f, %.2f, %.4f",
        x, y, z, roll, pitch, yaw);
 
    } catch (const tf2::TransformException & ex) {
      RCLCPP_ERROR(this->get_logger(), "TF异常: %s", ex.what()); 
    }
  }
 
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
  rclcpp::TimerBase::SharedPtr timer_;
};
 
int main(int argc, char * argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<TFSubscriber>());
  rclcpp::shutdown();
  return 0;
}